import numpy as np
import time
import os
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"
import cv2
cap = cv2.VideoCapture(1,cv2.CAP_MSMF)#打开内置摄像机
cap.set(cv2.CAP_PROP_FRAME_HEIGHT,1080)
cap.set(cv2.CAP_PROP_FRAME_WIDTH,3840)

class camera:
    def __init__(self,filename):
        self.imtx=np.loadtxt(filename)
        self.fx=self.imtx[0,0]
        self.fy=self.imtx[1,1]
        self.cx=self.imtx[0,2]
        self.cy=self.imtx[1,2]
        self.f=(self.fx+self.fy)/2
def corner(img):
    w=6
    h=4
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
    ret, corners = cv2.findChessboardCorners(gray, (w,h),None)
    if ret == True:
        cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
    return corners
L=camera("L.txt")
R=camera("R.txt")
print(L.f,R.f)
while cap.isOpened():#当摄像头打开时
    try:
        ret,frame=cap.read()#读取当前摄像头画面
        l=frame[:,0:1920]
        r=frame[:,1921:3840]
        #cv2.imshow('r',r)#显示当前摄像头画面q
        #cv2.waitKey(1)
        l_corners=corner(l)
        cv2.drawChessboardCorners(l, (6,4), l_corners, True)
        cv2.imshow('l',l)
        r_corners=corner(r)
        cv2.drawChessboardCorners(r, (6,4), r_corners, True)
        cv2.imshow('r',r)
        cv2.waitKey(1)
        a=(l_corners[0,0,0]-L.cx,l_corners[0,0,1]-L.cy)
        b=(r_corners[0,0,0]-R.cx,l_corners[0,0,1]-R.cy)
        print('red',a,b)
        print(120/(a[0]/L.f-b[0]/R.f))

        a=(l_corners[23,0,0]-L.cx,l_corners[23,0,1]-L.cy)
        b=(r_corners[23,0,0]-R.cx,l_corners[23,0,1]-R.cy)
        print('green',a,b)
        print(120/(a[0]/L.f-b[0]/R.f))
    except:
        pass